#ifndef ROBOT_H__
#define ROBOT_H__

#include "stdio.h"
#include "stdint.h"
#include "pca9685.h"
#include "FKIK.h"
#include "genki_pin.h"

#include "ohos_init.h"
#include "cmsis_os2.h"

#define KServoMax 180         // 髋关节舵机
#define KServoMin 45
#define TServoMax 150         // 大腿舵机
#define TServoMin 45
#define SServoMax 90          // 小腿舵机
#define SServoMin 15

// 摆线参数
#define XS      -19.0   // X原点
#define YS      25.4    // Y原点
#define ZS      75.0    // Z原点

#define XMOVE   30      // X距离
#define H       20      // 抬腿高度

//uint8_t RU[3] = {1, 2, 3 };     //右前腿编号
//uint8_t RB[3] = {6, 5, 4 };     //右后腿编号
//uint8_t LU[3] = {9, 10,11};     //左前腿编号
//uint8_t LB[3] = {14,13,12};     //左后腿编号

//uint8_t KJ[4] = {1,6,9, 14};    //髋关节
//uint8_t XJ[4] = {2,5,10,13};    //膝关节
//uint8_t HJ[4] = {3,4,11,12};    //踝关节

void Robot_init(void);
void Robot_PowerOFF(void);
void Robot_LegUnload(uint8_t legNum);

void Robot_FK_ResetPose(void);
void Robot_FK_GetDown(void);
void Robot_IK_ResetPose(void);
void Robot_Action_0(void);


/* ------------------ 单腿控制 ------------------ */
void FK_LUMove(float posk,float posx,float posh,uint16_t Time);   //左前腿
void FK_LBMove(float posk,float posx,float posh,uint16_t Time);   //左后腿
void FK_RUMove(float posk,float posx,float posh,uint16_t Time);   //右前腿
void FK_RBMove(float posk,float posx,float posh,uint16_t Time);   //右后腿

uint8_t FK_LegServoDebug(float Angle[],uint8_t cmd,uint8_t legNum);     // 单腿舵机校准

//float IK_RUPoint[3] = {};             // 逆解坐标点存储
//float IK_LUPoint[3] = {};
//float IK_RBPoint[3] = {};
//float IK_LBPoint[3] = {};

void IK_LUMove(float x,float y,float z,uint16_t Time);             //左前腿
void IK_LBMove(float x,float y,float z,uint16_t Time);             //左后腿
void IK_RUMove(float x,float y,float z,uint16_t Time);             //右前腿
void IK_RBMove(float x,float y,float z,uint16_t Time);             //右后腿

void IK_LegMove(float Point[],uint8_t LEGNum,uint16_t Time,uint8_t Print);
void LegPointDebug(float point[],uint8_t cmd,uint8_t PrintPoint,uint8_t PrintAngle);

/* ------------------ 姿态控制 ------------------ */
void PosToPoint(float x,float y,float z,uint16_t Time);   // 姿态坐标位置

/* ------------------ 步态控制 ------------------ */
void Trot(uint8_t step,uint8_t dir);
void TrotRL(uint8_t step,uint8_t dir);
void TrotTurn(uint8_t step,uint8_t dir);

#endif